package com.guazi.trackingtest.filter;

import com.guazi.trackingtest.Constants;
import com.guazi.trackingtest.PackData;
import com.guazi.trackingtest.cal.data.Point3D;

import java.util.ArrayList;
import java.util.List;

public class Kalman {

    private Double[] noiseVariances;       // Noise variances
    private Double[] predictedVariances;   // Predicted variances
    private Double[] predictedValues;      // Predicted values

    private boolean isInitialised;
    private PackData mSensorSingleData;
    private Point3D mPoint3D;

    public Kalman() {
        initObjects();
    }

    private void initObjects() {
        noiseVariances = new Double[3];
        predictedVariances = new Double[3];
        predictedValues = new Double[3];
        isInitialised = false;
    }

    public PackData filter(PackData packData) {
        this.mSensorSingleData = packData;
        List<Double> values = new ArrayList<Double>();
        values.add(packData.x);
        values.add(packData.y);
        values.add(packData.z);
        return process(values);
    }

    public void setNoise(double[] noise) {
        for (int i = 0; i < noise.length; i++) {
            noiseVariances[i] = noise[i];
        }
    }

    private PackData init(List<Double> initValues) {
        for (int i = 0; i < initValues.size(); i++) {
//            noiseVariances[i] = Constants.VARIANCE;
            predictedVariances[i] = noiseVariances[i];
            predictedValues[i] = initValues.get(i);
        }
        isInitialised = true;

        mSensorSingleData.setX(initValues.get(0));
        mSensorSingleData.setY(initValues.get(1));
        mSensorSingleData.setZ(initValues.get(2));

        return mSensorSingleData;
    }

    private PackData process(List<Double> measurementValues) {
        if (isInitialised) {
            List<Double> correctedValues = new ArrayList<Double>();
            for (int i = 0; i < measurementValues.size(); i++) {
                // compute the Kalman gain
                double kalmanGain = predictedVariances[i] / (predictedVariances[i] + noiseVariances[i]);

                // update the sensor prediction with the measurement
                double correctedValue = Constants.FILTER_GAIN * predictedValues[i] + (1.0 - Constants.FILTER_GAIN) *
                        measurementValues.get(i) + kalmanGain * (measurementValues.get(i) - predictedValues[i]);

                // predict next variance and value
                predictedVariances[i] = predictedVariances[i] * (1.0 - kalmanGain);
                predictedValues[i] = correctedValue;

                correctedValues.add(i, correctedValue);
            }

            mSensorSingleData.setX(correctedValues.get(0));
            mSensorSingleData.setY(correctedValues.get(1));
            mSensorSingleData.setZ(correctedValues.get(2));

            return mSensorSingleData;
        } else {
            return init(measurementValues);
        }
    }

    public Point3D filter(Point3D point3D){
        this.mPoint3D = point3D;
        List<Double> values = new ArrayList<Double>();
        values.add(point3D.x);
        values.add(point3D.y);
        values.add(point3D.z);
        return process2(values);
    }

    private Point3D process2(List<Double> measurementValues){
        if (isInitialised) {
            List<Double> correctedValues = new ArrayList<Double>();
            for (int i = 0; i < measurementValues.size(); i++) {
                // compute the Kalman gain
                double kalmanGain = predictedVariances[i] / (predictedVariances[i] + noiseVariances[i]);

                // update the sensor prediction with the measurement
                double correctedValue = Constants.FILTER_GAIN * predictedValues[i] + (1.0 - Constants.FILTER_GAIN) *
                        measurementValues.get(i) + kalmanGain * (measurementValues.get(i) - predictedValues[i]);

                // predict next variance and value
                predictedVariances[i] = predictedVariances[i] * (1.0 - kalmanGain);
                predictedValues[i] = correctedValue;

                correctedValues.add(i, correctedValue);
            }

            mPoint3D.x = correctedValues.get(0);
            mPoint3D.y = correctedValues.get(1);
            mPoint3D.z = correctedValues.get(2);

            return mPoint3D;
        } else {
            return init2(measurementValues);
        }
    }

    private Point3D init2(List<Double> initValues) {
        for (int i = 0; i < initValues.size(); i++) {
//            noiseVariances[i] = Constants.VARIANCE;
            predictedVariances[i] = noiseVariances[i];
            predictedValues[i] = initValues.get(i);
        }
        isInitialised = true;

        mPoint3D.x = initValues.get(0);
        mPoint3D.y = initValues.get(1);
        mPoint3D.z = initValues.get(2);

        return mPoint3D;
    }

    public void setNoise(Point3D point3D){
        noiseVariances[0] = point3D.x;
        noiseVariances[1] = point3D.y;
        noiseVariances[2] = point3D.z;
    }

    public boolean isInitialised(){
        return isInitialised;
    }
}
